//
// Created by Hoyin on 2024/4/16.
//

#include "PartiMatcher.h"

void
PartiMatcher::_calcExtendedROIs(const std::vector<cv::Rect> &blocks, uint32_t roiIdx, uint32_t hParts, uint32_t vParts,
                                std::vector<cv::Rect> &extended_rois_dst) {
	std::vector<cv::Rect> extended_rois;
	std::vector<uint32_t> extended_roi_indexes = _extendedROIs(int(roiIdx), int(hParts), int(vParts));
	for (uint32_t ri : extended_roi_indexes) {
		extended_rois.push_back(blocks.at(ri));
	}
	extended_rois_dst.assign(extended_rois.begin(), extended_rois.end());
}

std::vector<uint32_t> PartiMatcher::_extendedROIs(int roiIdx, int columns, int rows) {
	std::vector<uint32_t> extended_roi_indexes;
	if (roiIdx >= columns * rows) return extended_roi_indexes;
	int roiRow = roiIdx / columns;
	int roiColumn = roiIdx % columns;
//		std::cout << roiRow << ", " << roiColumn << std::endl;
	for (int i = -1; i <= 1; i++) {
		if (roiRow == 0 && i == -1) continue;
		if (roiRow == rows - 1 && i == 1) continue;
		for (int j = -1; j <= 1; j++) {
			if (roiColumn == 0 && j == -1) continue;
			if (roiColumn == columns - 1 && j == 1) continue;
			int eri = (roiRow + i) * columns + roiColumn + j;
			if (eri < 0 || eri > columns*rows) continue;
			extended_roi_indexes.push_back(eri);
		}
	}
	return extended_roi_indexes;
}

bool PartiMatcher::pointInBlocks(const cv::Point &interestPoint, const std::vector<cv::Rect> &rois) {
	bool flag = false;
	for (const cv::Rect &block : rois) {
		flag = interestPoint.inside(block) || flag;
		if (flag) return flag;
	}
	return flag;
}

cv::Mat PartiMatcher::calcMask(const std::vector<cv::KeyPoint> &keypoints1, const std::vector<cv::KeyPoint> &keypoints2,
                               const std::vector<cv::Rect> &blocks, uint32_t roiIdx, uint32_t hParts, uint32_t vParts) {
	cv::Mat mask(int(keypoints1.size()), int(keypoints2.size()), CV_8UC1, cv::Scalar(0));
	const cv::Rect& roi = blocks.at(roiIdx);
	std::vector<cv::Rect> extendedROIs;
	_calcExtendedROIs(blocks, roiIdx, hParts, vParts, extendedROIs);
	for (size_t kp1_idx = 0; kp1_idx < keypoints1.size(); kp1_idx++) {
		cv::KeyPoint kp1 = keypoints1.at(kp1_idx);
		for (size_t kp2_idx = 0; kp2_idx < keypoints2.size(); kp2_idx++) {
			cv::KeyPoint kp2 = keypoints2.at(kp2_idx);
			if (kp1.pt.inside(roi) && kp2.pt.inside(roi)) {
				mask.at<uchar>(int(kp1_idx), int(kp2_idx)) = 255;
			}
		}
	}
	return mask;
}

void PartiMatcher::match(const std::vector<cv::KeyPoint> &keypoints1, const std::vector<cv::KeyPoint> &keypoints2,
                         const cv::Mat &descriptor1, const cv::Mat &descriptor2, const std::vector<cv::Rect> &blocks,
                         std::vector<cv::DMatch> &matches, const cv::Mat &src1, const cv::Mat &src2, uint32_t hParts,
                         uint32_t vParts) {
	for (size_t i = 0; i < blocks.size(); i++) {
		std::vector<cv::DMatch> _matches;
		cv::Mat mask = calcMask(keypoints1, keypoints2, blocks, i, hParts, vParts);
		_matcher.match(descriptor1, descriptor2, _matches, mask);

		//	使用汉明距离过滤一部分，找出所有匹配结果中的最大hamming距离和最小hamming距离
		double min_dist = 1000, max_dist = 0;
		for (int i = 0; i < _matches.size(); i++) {
			double dist = _matches[i].distance;
			if (dist < min_dist) min_dist = dist;
			if (dist > max_dist) max_dist = dist;
		}
//		std::cout << "mindist: " << min_dist << std::endl;
		if (min_dist > _minDistThreshold) continue;
		std::vector<cv::DMatch> good_matches;
		for (int i = 0; i < _matches.size(); i++) {
			if (_matches[i].distance <= cv::max(2*min_dist, _minDistThreshold*2)) {
				good_matches.push_back(_matches[i]);
			}
		}

//		std::cout << "good_matches: " << good_matches.size() << std::endl;
		if (good_matches.size() < 4) continue;

		//	使用RANSAC筛选结果
		std::vector<cv::DMatch> good_ransac;
		utl::ransac(good_matches, keypoints1, keypoints2, good_ransac);

		matches.insert(matches.end(), good_ransac.begin(), good_ransac.end());
	}
	cv::Mat oImage;
	cv::drawMatches(src1, keypoints1, src2, keypoints2, matches, oImage);
//			cv::pyrDown(oImage, oImage, oImage.size()/2);
//			utl::showImage("PartiMatcher.match_deprecated", oImage);
	cv::imwrite("matches_pm.jpg", oImage);

}
